﻿//TODO: Test

using System;
using System.Drawing;
using System.Drawing.Imaging;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace LowLevelGraphics.Filter
{
    /// <summary>
    /// HarrisPlessey corner detection / harris corner detection
    /// </summary>
    public class HarrisPlessey : BaseImageFilter, ICornerFinder
    {
        protected float m_fK = 0.04f;
        protected float m_fThreshold = 1000f;
        protected double m_dSigma = 1.4;
        protected int m_nR = 3;
        protected List<Point> m_aPoint = new List<Point>();

        // Convolution filters for image derivatives
        protected OwnOptimizedConvolutionInt m_ConvolutionX = null;
        protected OwnOptimizedConvolutionInt m_ConvolutionY = null;
        protected Color m_ColorToSet = Definitions.Red;

        /// <summary>
        ///   Harris parameter k. Default value is 0.04.
        /// </summary>
        public float K
        {
            get { return m_fK; }
            set { m_fK = value; }
        }

        /// <summary>
        ///   Harris threshold. Default value is 1000.
        /// </summary>
        public float Threshold
        {
            get { return m_fThreshold; }
            set { m_fThreshold = value; }
        }

        /// <summary>
        ///   Gaussian smoothing sigma. Default value is 1.4.
        /// </summary>
        public double Sigma
        {
            get { return m_dSigma; }
            set { m_dSigma = value; }
        }

        /// <summary>
        ///   Non-maximum suppression window radius. Default value is 3.
        /// </summary>
        public int Suppression
        {
            get { return m_nR; }
            set { m_nR = value; }
        }

        /// <summary>
        /// Gets the points
        /// </summary>
        public List<Point> Points
        {
            get { return m_aPoint; }
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="HarrisPlessey"/> class.
        /// </summary>
        public HarrisPlessey()
        {
            this.m_ConvolutionX = new OwnOptimizedConvolutionInt(new int[,]
                {
                    { -1,  0,  1, },
                    { -1,  0,  1, },
                    { -1,  0,  1  } 
                }
            );

            this.m_ConvolutionY = new OwnOptimizedConvolutionInt(new int[,] 
                {
                    { -1, -1, -1, },
                    {  0,  0,  0, },
                    {  1,  1,  1  } 
                }
            );
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="HarrisPlessey"/> class.
        /// </summary>
        /// <param name="k">The k.</param>
        public HarrisPlessey(float k)
            : this()
        {
            this.m_fK = k;
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="HarrisPlessey"/> class.
        /// </summary>
        /// <param name="k">The k.</param>
        /// <param name="threshold">The threshold.</param>
        public HarrisPlessey(float k, float threshold)
            : this()
        {
            this.m_fK = k;
            this.m_fThreshold = threshold;
        }

        /// <summary>
        ///   Initializes a new instance of the <see cref="HarrisPlessey"/> class.
        /// </summary>
        public HarrisPlessey(float k, float threshold, double sigma)
            : this()
        {
            this.m_fK = k;
            this.m_fThreshold = threshold;
            this.m_dSigma = sigma;
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="_harrisPlessey"></param>
        internal HarrisPlessey(HarrisPlessey _harrisPlessey)
        {
            this.m_fK = _harrisPlessey.m_fK;
            this.m_fThreshold = _harrisPlessey.m_fThreshold;
            this.m_dSigma = _harrisPlessey.m_dSigma;
        }

        /// <summary>
        /// Execute this Filter
        /// A Debug Statement is written if a concrete Filter is called
        /// </summary>
        /// <param name="_bitmap"></param>
        /// <returns></returns>
        public override UnsafeBitmap Execute(UnsafeBitmap _bitmap)
        {
            // 1. Calculate partial differences
            UnsafeBitmap grayImage = new GrayScale().Execute((UnsafeBitmap)_bitmap.Clone());
            int width = _bitmap.Width;
            int height = _bitmap.Height;
            UnsafeBitmap diffx = new UnsafeBitmap(new Bitmap(width, height));
            UnsafeBitmap diffy = new UnsafeBitmap(new Bitmap(width, height));
            UnsafeBitmap diffxy = new UnsafeBitmap(new Bitmap(width, height));

            // get source image size
            diffx = m_ConvolutionX.ExecuteCloned(grayImage);
            diffy = m_ConvolutionY.ExecuteCloned(grayImage);
            diffx = m_ConvolutionY.ExecuteCloned(diffx);

            // 2. Smooth the diff images
            if (m_dSigma > 0.0)
            {
                GaussianBlur blur = new GaussianBlur((int)m_dSigma);
                blur.Execute(diffx);
                blur.Execute(diffy);
                blur.Execute(diffxy);
            }

            // 3. Compute Harris Corner Response
            float[,] H = new float[height, width];

            unsafe
            {            
                float M, A, B, C;

                for (int y = 0; y < height; y++)
                {
                    for (int x = 0; x < width; x++)
                    {
                        Color color = _bitmap.GetPixel(x, y);
                        A = color.R;
                        B = color.G;
                        C = color.B;

                        // Harris corner measure
                        M = (A * B - C * C) - (m_fK * ((A + B) * (A + B)));

                        if (M > m_fThreshold)
                        {
                            H[y, x] = M;
                        }
                        else
                        {
                            H[y, x] = 0;
                        }
                    }
                }
            }

            // Free resources
            diffx.Dispose();
            diffy.Dispose();
            diffxy.Dispose();

            // 4. Supress non-maximum points

            // for each row
            for (int y = m_nR, maxY = height - m_nR; y < maxY; y++)
            {
                // for each pixel
                for (int x = m_nR, maxX = width - m_nR; x < maxX; x++)
                {
                    double currentValue = H[y, x];

                    // for each windows' row
                    for (int i = -m_nR; (currentValue != 0) && (i <= m_nR); i++)
                    {
                        // for each windows' pixel
                        for (int j = -m_nR; j <= m_nR; j++)
                        {
                            if (H[y + i, x + j] > currentValue)
                            {
                                currentValue = 0;
                                break;
                            }
                        }
                    }

                    // check if this point is really interesting
                    if (currentValue != 0)
                    {
                        m_aPoint.Add(new Point(x, y));
                        _bitmap.SetPixel(x, y, m_ColorToSet);
                    }
                }
            }

            return _bitmap;
        }

        /// <summary>
        /// This is used to clone a filter, each filter must implement it's own clone functionality
        /// </summary>
        /// <returns></returns>
        public override object Clone()
        {
            return new HarrisPlessey(this);
        }
    }
}
